#include <stdlib.h>
#include <string.h>    // strncpy()
#include <sys/stat.h>  // struct stat
#include <unistd.h>    // stat()
#include <limits.h>    // PATH_MAX
#include <dlfcn.h>
#include <map>
#include "timer.hh"
#include "rtapi/rtapi_limits.h"  // PATH_MAX
#include <stdlib.h>
#include <string.h>    // strncpy()
#include <sys/stat.h>  // struct stat
#include <unistd.h>    // stat()

#include "nml_intf/canon.hh"  // CANON_VECTOR, GET_PROGRAM_ORIGIN()
#include "inifile.hh"
#include "nml_intf/interp_return.hh"  // INTERP_FILE_NOT_OPEN
#include "nml_intf/interpl.hh"        // NML_INTERP_LIST, interp_list, waitFlag
#include "rcs.hh"                     // INIFILE
#include "rcs_print.hh"
#include "rs274ngc/rs274ngc_interp.hh"  // the interpreter
#include "task.hh"                      // zucTaskCommand etc
#include "nml_intf/zuc.hh"              // ZUC NML
#include "nml_intf/zuc_nml.hh"
#include "nml_intf/zucglb.h"  // ZUC_INIFILE
//#include "taskclass.hh"
#include "errcode.h"
#include "inifile.hh"
#include "motion/motion.h"
#include "log/zuclog.h"
#include "log/datacollector.hh"
#include "rtdev/safetyboard/scbitf.h"
#include "task/motion_al.h"
#include "zuc/task/taskintf.h"

#define USER_DEFINED_FUNCTION_MAX_DIRS 5
#define MAX_M_DIRS (USER_DEFINED_FUNCTION_MAX_DIRS + 1)
// note:the +1 is for the PROGRAM_PREFIX or default directory==nc_files

/* flag for how we want to interpret traj coord mode, as mdi or auto */
static int mdiOrAuto = ZUC_TASK_MODE_AUTO;

InterpBase* pinterp = 0;
#define interp (*pinterp)
setup_pointer _is = 0;  // helper for gdb hardware watchpoints FIXME
/*
 format string for user-defined programs, e.g., "programs/M1%02d" means
 user-defined programs are in the programs/ directory and are named
 M1XX, where XX is a two-digit string.
 */
static char user_defined_fmt[MAX_M_DIRS][ZUC_SYSTEM_CMD_LEN];  // ex: "dirname/M1%02d"

// index to directory for each user defined function:
static int user_defined_function_dirindex[USER_DEFINED_FUNCTION_NUM];

// recode script file name and current line number before report error and close
// interp printf(">>>>>>>>>>scriptFile:%s\n", interpPtr->_setup.filename);
// printf(">>>>>>Script Error. errlineNum = %d  errMsg = \"%s\"\n",
// interpPtr->sequence_number(), interpPtr->getSavedError());
// Interp* interpPtr = dynamic_cast<Interp*>(pinterp);
int LogScriptInfo(Interp* interpPtr)
{
    if (!interpPtr)
    {
        return -1;
    }
    if (strlen(interpPtr->_setup.filename) > strlen(ZUC_SCRIPT_DIR "/program/"))
    {
        zucLogInfo(0xFFFFFF,
                   "Script Error. Line:%d  File: \"%s\"\n",
                   interpPtr->_setup.current_line,
                   &(interpPtr->_setup.filename[strlen(ZUC_SCRIPT_DIR "/program/") + 1]));
    }
    else
    {
        zucLogInfo(0xFFFFFF, "Script Error. errLine=%d  errMsg= \"%s\"\n", interpPtr->_setup.current_line, interpPtr->_setup.filename);
    }

    return 0;
}

std::string get_md5sum(const std::string& filename)
{
    std::string md5string = "0";
    std::string cmd = "md5sum \"" + filename + "\" | cut -d ' ' -f 1";
    FILE* pipe = popen(cmd.c_str(), "r");
    if (pipe != NULL)
    {
        static const unsigned md5size = 32;
        unsigned char md5res[md5size + 1];
        int readSize = fread((void*)md5res, sizeof(char), md5size, pipe);
        pclose(pipe);
        if (readSize != md5size)
        {
            printf("Error reading md5 of %s\n", filename.c_str());
            return md5string;
        }
        md5res[md5size] = 0;
        md5string.assign(&md5res[0], &md5res[md5size]);
    }
    return md5string;
}

static void user_defined_add_m_code(int num, double arg1, double arg2)
{
    // num      is the m_code number, typically 00-99 corresponding to M100-M199
    char fmt[ZUC_SYSTEM_CMD_LEN];
    ZUC_SYSTEM_CMD system_cmd;

    //we call FINISH() to flush any linked motions before the M1xx call,
    //otherwise they would mix badly
    FINISH();
    strcpy(fmt, user_defined_fmt[user_defined_function_dirindex[num]]);
    strcat(fmt, " %f %f");
    snprintf(system_cmd.string, sizeof(system_cmd.string), fmt, num, arg1, arg2);
    interp_list.append(system_cmd);
    rcs_print("user_defined_add_m_code: interplist->append!\n");
}

int zucTaskInit()
{
    char mdir[MAX_M_DIRS][PATH_MAX + 1];
    int num, dct, dmax;
    char path[ZUC_SYSTEM_CMD_LEN];
    struct stat buf;
    IniFile inifile;
    const char* inistring;

    inifile.Open(zuc_inifile);

    // Identify user_defined_function directories
    if (NULL != (inistring = inifile.Find("PROGRAM_PREFIX", "DISPLAY")))
    {
        strncpy(mdir[0], inistring, sizeof(mdir[0]));
        if (mdir[0][sizeof(mdir[0]) - 1] != '\0')
        {
            rcs_print("[DISPLAY]PROGRAM_PREFIX too long (max len %zu)\n", sizeof(mdir[0]));
            return -1;
        }
    }
    else
    {
        // default dir if no PROGRAM_PREFIX
        strncpy(mdir[0], "nc_files", sizeof(mdir[0]));
        if (mdir[0][sizeof(mdir[0]) - 1] != '\0')
        {
            rcs_print("default nc_files too long (max len %zu)\n", sizeof(mdir[0]));
            return -1;
        }
    }
    dmax = 1;  //one directory mdir[0],  USER_M_PATH specifies additional dirs

    // user can specify a list of directories for user defined functions
    // with a colon (:) separated list
    if (NULL != (inistring = inifile.Find("USER_M_PATH", "RS274NGC")))
    {
        char* nextdir;
        char tmpdirs[PATH_MAX];

        for (dct = 1; dct < MAX_M_DIRS; dct++) mdir[dct][0] = 0;

        strncpy(tmpdirs, inistring, sizeof(tmpdirs));
        if (tmpdirs[sizeof(tmpdirs) - 1] != '\0')
        {
            rcs_print("[RS274NGC]USER_M_PATH too long (max len %zu)\n", sizeof(tmpdirs));
            return -1;
        }

        nextdir = strtok(tmpdirs, ":");  // first token
        dct = 1;
        while (dct < MAX_M_DIRS)
        {
            if (nextdir == NULL)
                break;  // no more tokens
            strncpy(mdir[dct], nextdir, sizeof(mdir[dct]));
            if (mdir[dct][sizeof(mdir[dct]) - 1] != '\0')
            {
                rcs_print("[RS274NGC]USER_M_PATH component (%s) too long (max len %zu)\n", nextdir, sizeof(mdir[dct]));
                return -1;
            }
            nextdir = strtok(NULL, ":");
            dct++;
        }
        dmax = dct;
    }
    inifile.Close();

    /* check for programs named programs/M100 .. programs/M199 and add
	 any to the user defined functions list */
    for (num = 0; num < USER_DEFINED_FUNCTION_NUM; num++)
    {
        for (dct = 0; dct < dmax; dct++)
        {
            char expanddir[LINELEN];
            if (!mdir[dct][0])
                continue;
            if (inifile.TildeExpansion(mdir[dct], expanddir, sizeof(expanddir)))
            {
                rcs_print("zucTaskInit: TildeExpansion failed for %s, ignoring\n", mdir[dct]);
            }
            snprintf(path, sizeof(path), "%s/M1%02d", expanddir, num);
            if (0 == stat(path, &buf))
            {
                if (buf.st_mode & S_IXUSR)
                {
                    // set the user_defined_fmt string with dirname
                    // note the %%02d means 2 digits after the M code
                    // and we need two % to get the literal %
                    snprintf(user_defined_fmt[dct], sizeof(user_defined_fmt[0]), "%s/M1%%02d", expanddir);  // update global
                    USER_DEFINED_FUNCTION_ADD(user_defined_add_m_code, num);
                    if (zuc_debug & ZUC_DEBUG_CONFIG)
                    {
                        rcs_print("zucTaskInit: adding user-defined function %s\n", path);
                    }
                    user_defined_function_dirindex[num] = dct;
                    break;  // use first occurrence found for num
                }
                else
                {
                    if (zuc_debug & ZUC_DEBUG_CONFIG)
                    {
                        rcs_print("zucTaskInit: user-defined function %s found, but not executable, so ignoring\n", path);
                    }
                }
            }
        }
    }

    return 0;
}

int zucTaskHalt() { return 0; }

int zucTaskAbort()
{
    zucMotionAbort();
    //remove("/tmp/crossfilemsg");
    // clear out the pending command
    zucTaskCommand = 0;
    interp_list.clear();

    // clear out the interpreter state
    zucStatus->task.interpState = ZUC_TASK_INTERP_IDLE;
    zucStatus->task.execState = ZUC_TASK_EXEC_DONE;
    zucStatus->task.task_paused = 0;
    zucStatus->task.motionLine = 0;
    zucStatus->task.readLine = 0;
    zucStatus->task.sleepLine = 0;
    zucStatus->task.command[0] = 0;
    zucStatus->task.callLevel = 0;
    zucStatus->stepStatus.isSteping = 0;
    stepping = 0;
    steppingWait = 0;

    // now queue up command to resynch interpreter
    ZUC_TASK_PLAN_SYNCH taskPlanSynchCmd;
    zucTaskQueueCommand(&taskPlanSynchCmd);
    // without zucTaskPlanClose(), a new run command resumes at
    // aborted line-- feature that may be considered later
    {
        int was_open = taskplanopen;
        zucTaskPlanClose();
        if (zuc_debug & ZUC_DEBUG_INTERP && was_open)
        {
            rcs_print("zucTaskPlanClose() called at %s:%d\n", __FILE__, __LINE__);
        }
    }

    for (int i = 0; i < MAX_SUB_THREAD; ++i)
    {
        threadInterpList[i].thread_id = 0;
        threadInterpList[i].pause_flag = 0;
        threadInterpList[i].join_flag = 0;
    }

    return 0;
}

int zucTaskSetMode(int mode)
{
    int retval = 0;
    switch (mode)
    {
    case ZUC_TASK_MODE_MANUAL:
        zucTrajSetMode(ZUC_TRAJ_MODE_FREE);
        mdiOrAuto = ZUC_TASK_MODE_MANUAL;  // we'll default back to here
        break;

    case ZUC_TASK_MODE_MDI:
        zucTrajSetMode(ZUC_TRAJ_MODE_COORD);
        zucTaskAbort();
        zucTaskPlanSynch();
        mdiOrAuto = ZUC_TASK_MODE_MDI;
        break;

    case ZUC_TASK_MODE_AUTO:
        if (zucStatus->task.mode != ZUC_TASK_MODE_AUTO)
        {
            zucTrajSetMode(ZUC_TRAJ_MODE_COORD);
            zucTaskAbort();
            zucTaskPlanSynch();
            mdiOrAuto = ZUC_TASK_MODE_AUTO;
        }
        break;

    case ZUC_TASK_MODE_DRAG:
        if (zucStatus->currSimMode)
        {
            zucOperatorError(DRAG_CANNOT_ENABLE_ON_SIM, "can not enter drag mode while sim mode");
        }
        else
        {
            zucTrajSetMode(ZUC_TRAJ_MODE_DRAG);
            mdiOrAuto = ZUC_TASK_MODE_DRAG;
        }
        break;

    default:
        retval = -1;
        break;
    }

    return retval;
}

int zucTaskSetState(int state, int robid)
{
    int t;
    int retval = 0;
    auto task_set_state = mot::al::MotionProxy::instance().robot(0);
    if (!task_set_state.get())
    {
        return -1;
    }

    switch (state)
    {
    case ZUC_TASK_STATE_OFF:  // servo disable
    {
        return zucTrajDisable(robid);
        // if (zucStatus->task.interpState != ZUC_TASK_INTERP_IDLE)
        // {
        //     zucOperatorError(CANNOT_SERVO_OFF_WHEN_PROG_RUN, "cannot disable the robot when a program is running");
        //     return -1;
        // }
        // if (false == zucStatus->motion.traj.inpos)
        // {
        //     zucOperatorError(CANNOT_SERVO_OFF_WHEN_ROBOT_MOVING, "cannot disable servo when robot is moving");
        //     return -1;
        // }
        // task_set_state->zucMotionAbort();
        // for (t = 0; t < zucStatus->motion.traj.joints; t++) { zucJointDisable(t); }
        // zucMotorsServoOn(false);
        // zucTaskAbort();
        // zucJointUnhome(-2);  // only those joints which are volatile_home
        // zucAbortCleanup(ZUC_ABORT_TASK_STATE_OFF);
        // zucTaskPlanSynch();
        break;
    }
    case ZUC_TASK_STATE_ON:  // servo enable
    {
        return zucTrajEnable(robid);

        break;
    }
    case ZUC_TASK_STATE_POWERED_ON: {
        int err = zucRobotPowerOn(robid);
        if (err)
        {
            return err;
        }
        break;
        // if (zucStatus->cab_type == 1)  //v1.7.1后不支持cabv1
        // {
        //     zucOperatorError(UNSUPPORTED_CABINET, "unsupport cabv1#{\"fixed_key\":[\"CabV1\"]}");
        //     return -1;
        // }
        // if (1 == zucStatus->motion.emergency_stop)
        // {
        //     zucOperatorWarning(INFO_EMERGENCY_STOP_OCCURS_CANNOT_POWER_ON, "When an emergency stop occurs, the power-on operation cannot be performed.");
        //     return -1;
        // }
        // if (!zucStatus->motion.powered_on)
        // {
        //     if (true == zucStatus->motion.func_io_req.power_off_req)
        //     {  // funcio power_off_req  is  true.
        //         zucOperatorError(ERR_FDI_POWER_ON_LOCKED, "robot power on  function is locked by power-off-request of funcIO.");
        //         return -1;
        //     }
        //     zucMotionEstopOff();
        //     if (zucStatus->motion.category_one_stop_program_pause == 0)
        //     {
        //         zucTaskAbort();
        //         zucAbortCleanup(ZUC_ABORT_TASK_STATE_POWERD_ON);
        //         zucTaskPlanSynch();
        //     }
        // }
        // else
        // {
        //     zucOperatorError(INFO_ROBOT_HAS_BEEN_POWERED_ON, "The robot has been powered on .");
        //     return -1;
        // }
    }
    case ZUC_TASK_STATE_POWERED_OFF: {
        int err = zucRobotPowerOff(robid);
        if (err)
        {
            return err;
        }
        break;
    }
    default: {
        retval = -1;
        break;
    }
    }

    return retval;
}

// WM access functions

/*
 determineMode()

 Looks at mode of subsystems, and returns associated mode

 Depends on traj mode, and mdiOrAuto flag

 traj mode   mdiOrAuto     task mode
 ---------   ---------     ---------
 FREE        XXX           MANUAL
 TELEOP      XXX           MANUAL
 COORD       MDI           MDI
 COORD       AUTO          AUTO
 */
static int determineMode()
{
    // if(zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_DRAG)
    //     rcs_print("traj mode is now Drag mode!\n");

    // if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_FREE)
    // {
    //     return ZUC_TASK_MODE_MANUAL;
    // }
    // if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP)
    // {
    //     return ZUC_TASK_MODE_MANUAL;
    // }
    // if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP_TOOL)
    // {
    //     return ZUC_TASK_MODE_MANUAL;
    // }
    // if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_SERVOJOP)
    // {
    //     return ZUC_TASK_MODE_MANUAL;
    // }
    // if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_ADMITTANCE)
    // {
    //     return ZUC_TASK_MODE_MANUAL;
    // }

    // for ZUC_TRAJ_MODE_COORD
    return mdiOrAuto;
}

/*
 determineState()

 Looks at state of subsystems, and returns associated state

 Depends on traj enabled, io estop, and desired task state

 traj enabled   io estop      state
 ------------   --------      -----
 DISABLED       ESTOP         ESTOP
 ENABLED        ESTOP         ESTOP
 DISABLED       OUT OF ESTOP  ESTOP_RESET
 ENABLED        OUT OF ESTOP  ON
 */
static int determineState()
{
    if (!zucStatus->motion.powered_on)
    {
        return ZUC_TASK_STATE_POWERED_OFF;
    }

    if (!zucStatus->motion.is_enabled)
    {
        return ZUC_TASK_STATE_POWERED_ON;
    }

    return ZUC_TASK_STATE_ON;
}

//@brief: 指定单个分隔符（单个字符或子串）分割字符串
//@param: src 原字符串；delimiter 分隔符，单个字符或子串
vector<string> splitstr(const string& src, const string& delimiter)
{
    std::vector<string> vetstr;

    // 入参检查
    // 1.原字符串为空或等于分隔符，返回空 vector
    if (src == "" || src == delimiter)
    {
        return vetstr;
    }

    string::size_type startpos = 0;
    auto index = src.find(delimiter);
    while (index != string::npos)
    {
        auto str = src.substr(startpos, index - startpos);
        if (str != "")
        {
            vetstr.push_back(str);
        }
        startpos = index + delimiter.length();
        index = src.find(delimiter, startpos);
    }
    // 取最后一个子串
    auto str = src.substr(startpos);
    if (str != "")
    {
        vetstr.push_back(str);
    }
    return vetstr;
}

// //@brief：对输入的字符串进行json格式化
// //@param：str 输入的字符串
// //@output：格式化后的json字符串
// // demo:
// // input:fixed_key:0x123,0x122,0x321,0x321;param:0,1;others:md5,dafwev2113
// // output:
// //{"fixed_key":["0x123","0x122","0x321","0x321"],"param":{"0":"1"},"others":{"md5":"dafwev2113"}}
// string make_json(string str)
// {
//     // string str =
//     // "description:0.123,0122;reason:0111;param:0,1,2,3,4,5;others:file,aaa.jks,line,11,md5,md531212";
//     string str_inner_inner = "";
//     string str_inner = "";
//     string str_inner_mid = "";
//     string str_json = "";

//     auto vetstr_del_fen_hao = splitstr(str, ";");  // split with ";"  description:0.123,0122 reason:0111
//                                                    // action:9999 param:0,1 reason 0111

//     for (size_t k = 0; k < vetstr_del_fen_hao.size(); k++)
//     {
//         auto vetstr_del_mao_hao = splitstr(vetstr_del_fen_hao[k], ":");  // split with ":"  description 0.123,0122
//         string key = vetstr_del_mao_hao[0];

//         if (vetstr_del_mao_hao[1].size() == 1)
//         {
//             str_inner_inner = vetstr_del_mao_hao[1];
//         }
//         else
//         {
//             if (key == "param")
//             {
//                 str_inner_inner = "";
//                 auto vetstr_del_dou_hao = splitstr(vetstr_del_mao_hao[1], ",");  // split with ","
//                 if (vetstr_del_dou_hao.size() >= 2)
//                 {
//                     for (size_t i = 0; i < vetstr_del_dou_hao.size(); i++)
//                     {
//                         if ((i + 1) % 2 == 0)
//                         {
//                             if (i == vetstr_del_dou_hao.size() - 1)
//                             {
//                                 str_inner_inner = "{" + str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + "}";
//                             }
//                             else
//                             {
//                                 str_inner_inner = str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + ",";
//                             }
//                         }
//                         else
//                         {
//                             if (i == vetstr_del_dou_hao.size() - 1)
//                             {
//                                 str_inner_inner = "{" + str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + "}";
//                             }
//                             else
//                             {
//                                 str_inner_inner = str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + ":";
//                             }
//                         }
//                     }
//                 }
//                 else
//                 {
//                     for (size_t i = 0; i < vetstr_del_dou_hao.size(); i++)
//                     {
//                         if (i == vetstr_del_dou_hao.size() - 1)
//                         {
//                             str_inner_inner = "{" + str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + "}";
//                         }
//                         else
//                         {
//                             str_inner_inner = str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + ":";
//                         }
//                     }
//                     str_inner_inner = "\"" + vetstr_del_dou_hao[0] + "\"";
//                 }
//             }
//             else if (key == "others")
//             {
//                 str_inner_inner = "{";
//                 auto vetstr_del_dou_hao = splitstr(vetstr_del_mao_hao[1], ",");  // split with ","
//                 for (size_t i = 0; i < vetstr_del_dou_hao.size(); i++)
//                 {
//                     if (((i + 1) % 2 == 0) && ((i + 1) != vetstr_del_dou_hao.size()))
//                     {
//                         str_inner_inner = str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + ",";
//                     }
//                     else if (((i + 1) % 2 == 0) && ((i + 1) == vetstr_del_dou_hao.size()))
//                     {
//                         str_inner_inner = str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"";
//                     }
//                     else
//                     {
//                         str_inner_inner = str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + ":";
//                     }
//                 }
//                 str_inner_inner = str_inner_inner + "}";
//             }
//             else
//             {
//                 str_inner_inner = "";
//                 auto vetstr_del_dou_hao = splitstr(vetstr_del_mao_hao[1], ",");  // split with ","
//                 if (vetstr_del_dou_hao.size() > 1)
//                 {
//                     for (size_t i = 0; i < vetstr_del_dou_hao.size(); i++)
//                     {
//                         if (i == vetstr_del_dou_hao.size() - 1)
//                         {
//                             str_inner_inner = "[" + str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + "]";
//                         }
//                         else
//                         {
//                             str_inner_inner = str_inner_inner + "\"" + vetstr_del_dou_hao[i] + "\"" + ",";
//                         }
//                     }
//                 }
//                 else
//                 {
//                     str_inner_inner = "\"" + vetstr_del_dou_hao[0] + "\"";
//                 }
//             }
//         }
//         if (k == 0)
//         {
//             str_inner = "\"" + key + "\"" + ":" + str_inner_inner;
//             str_inner_mid = str_inner_mid + str_inner;
//         }
//         else
//         {
//             str_inner = "\"" + key + "\"" + ":" + str_inner_inner;
//             str_inner_mid = str_inner_mid + "," + str_inner;
//         }
//     }
//     str_json = "{" + str_inner_mid + "}";
//     return str_json;
// }

static char interp_error_text_buf[LINELEN];
static char interp_stack_buf[LINELEN];

static void print_interp_error(int retval)
{
    char dir[] = ZUC_SCRIPT_DIR;  //参考路径
    char filePath[PATH_MAX];
    strncpy(filePath,
            (dynamic_cast<Interp*>(pinterp))->_setup.filename,
            PATH_MAX);  // zucTaskAbort 会调用interp.close() ,如果有 脚本调用的话，会回退到主调脚本，丢失原本出错的脚本文件名。
    int errLine = (dynamic_cast<Interp*>(pinterp))->_setup.current_line;
    std::string md5 = get_md5sum(std::string(filePath));

    zucTaskAbort();
    int index = 0;
    int suberrcode = 0;
    if (retval == 0)
    {
        return;
    }

    if (0 != zucStatus)
    {
        zucStatus->task.interpreter_errcode = retval;
    }
    else
    {
        zucStatus->task.interpreter_errcode = 0;
    }
    interp_error_text_buf[0] = 0;
    interp.error_text(retval, &suberrcode, interp_error_text_buf, LINELEN);
    if (0 != interp_error_text_buf[0])
    {
        rcs_print_error("suberrcode: %d, interp_error: %s\n", suberrcode, interp_error_text_buf);
    }

    int errCode = 0;
    if (retval == INTERP_ERROR)
    {
        errCode = suberrcode;
    }
    else
    {
        errCode = PROGRAM_FILE_NOT_OPEN;
        sprintf(interp_error_text_buf, "failed to open '%s'\n", (dynamic_cast<Interp*>(pinterp))->_setup.filename);
    }

    printf("filePath:%s[%d],  errLine: %d \n", filePath, (int)strlen(filePath), errLine);
    if (strncmp(filePath, dir, strlen(dir)) == 0)
    {
        strncpy(filePath, &filePath[strlen(dir)], PATH_MAX - strlen(dir));  // 纪录相对于 ZUC_SCRIPT_DIR"/" 的文件路径。
    }

    if (strlen(filePath) <= 0 || md5.compare("0") == 0)
    {
        // zucOperatorError(errCode, "%s #{\"1\":\"%s\", \"2\":%d, \"check\":
        // \"%s\"}",  interp_error_text_buf,"",-1, "0");  // interp
        // 初始化等非执行脚本程序过程报错，文件名为空，行号为-1；
        //  文件名异常， 不追加json字段
        errCode = PROGRAM_FILE_FAILED_OPEN;
        // sprintf(interp_error_text_buf, "failed to open '%s'\n", filePath);
        zucOperatorError(errCode, "%s", interp_error_text_buf);
    }
    else
    {
        char seps[] = "#";
        char* token = NULL;
        token = strtok(interp_error_text_buf, seps);
        token = strtok(NULL, seps);
        if (token == NULL)
        {
            token = (char*)"";
        }
        //{"fixed_key":["0x123","0x122","0x321","0x321"],"param":{"0":"1"},"others":{"md5":"dafwev2113"}}
        std::string jsonStr = "{\"fixed_key\":[\"" + std::string(filePath) + "\",\"" + std::to_string(errLine) + "\"]," + std::string(token) +
                              "\"others\":{\"md5\":\"" + md5 + "\"}}";
        // std::string jsonStr = make_json(str);
        zucOperatorError(errCode, "%s #%s", interp_error_text_buf, jsonStr.c_str());
    }
    index = 0;
    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        // rcs_print("Interpreter stack: \t");
        while (index < 5)
        {
            interp_stack_buf[0] = 0;
            interp.stack_name(index, interp_stack_buf, LINELEN);
            if (0 == interp_stack_buf[0])
            {
                break;
            }
            rcs_print(" - %s ", interp_stack_buf);
            index++;
        }
        // rcs_print("\n");
    }
}

int zucTaskPlanInit()
{
    if (!pinterp)
    {
        IniFile inifile;
        const char* inistring;
        inifile.Open(zuc_inifile);
        if ((inistring = inifile.Find("INTERPRETER", "TASK")))
        {
            pinterp = interp_from_shlib(inistring);
            fprintf(stderr, "interp_from_shlib() -> %p\n", pinterp);
        }
        inifile.Close();
    }
    if (!pinterp)
    {
        pinterp = new Interp;
    }

    Interp* i = dynamic_cast<Interp*>(pinterp);
    if (i)
        _is = &i->_setup;  // FIXME
    else
        _is = 0;
    interp.ini_load(zuc_inifile);
    waitFlag = 0;
    interp._waitFlag = 0;
    waitTimeout = true;

    int retval = interp.init();
    if (retval > INTERP_MIN_ERROR)
    {  // I'd think this should be fatal.
        print_interp_error(retval);
    }
    else
    {
        if (0 != rs274ngc_startup_code[0])
        {
            retval = interp.execute(rs274ngc_startup_code);
            while (retval == INTERP_EXECUTE_FINISH) { retval = interp.execute(0); }
            if (retval > INTERP_MIN_ERROR)
            {
                print_interp_error(retval);
            }
        }
    }

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanInit() returned %d\n", retval);
    }
    return retval;
}

int zucTaskPlanSetWait()
{
    interp._waitFlag = 1;

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanSetWait() called\n");
    }

    return 0;
}

int zucTaskPlanIsWait() { return interp._waitFlag; }

int zucTaskPlanSetOptionalStop(bool state)
{
    SET_OPTIONAL_PROGRAM_STOP(state);
    return 0;
}

int zucTaskPlanSetBlockDelete(bool state)
{
    SET_BLOCK_DELETE(state);
    return 0;
}

int zucTaskPlanSynch()
{
    int retval = interp.synch();

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanSynch() returned %d\n", retval);
    }

    zucTrajSetRobotToolID(0, -1);
    zucTrajSetRobotToolID(1, -1);
    return retval;
}

int zucTaskPlanClearWait()
{
    interp._waitFlag = 0;

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanClearWait() called\n");
    }
    zucTaskPlanSynch();
    return 0;
}

void zucTaskPlanExit()
{
    if (pinterp != NULL)
    {
        interp.exit();
    }
}

void zucTaskSetStartLine(int startLine) { interp.set_prog_start_line(startLine); }

int zucTaskPlanOpen(const char* file)
{
    if (zucStatus != 0)
    {
        zucStatus->task.motionLine = 0;
        zucStatus->task.currentLine = 0;
        zucStatus->task.readLine = 0;
        zucStatus->task.sleepLine = 0;
        zucStatus->task.executingLine = 0;
    }
    interp_list.set_line_number(0);

    int retval = interp.open(file);
    if (retval > INTERP_MIN_ERROR)
    {
        print_interp_error(retval);
        return retval;
    }
    taskplanopen = 1;
    zucDiagSetLoadedProgram(file);
    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanOpen(%s) returned %d\n", file, retval);
    }

    for (int i = 0; i < MAX_SUB_THREAD; ++i) zucStatus->sub_thread_state[i][0] = '\0';
    thread_pause_flag = false;
    thread_exit_flag = false;
    threadInterpNum = 0;

    return retval;
}

int zucTaskPlanRead()
{
    interp.updateSteppingState(zucStatus->isSteppingMode);
    int retval = interp.read();
    if (retval == INTERP_FILE_NOT_OPEN)
    {
        if (zucStatus->task.file[0] != 0)
        {
            retval = interp.open(zucStatus->task.file);
            if (retval > INTERP_MIN_ERROR)
            {
                print_interp_error(retval);
                zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
            }
            retval = interp.read();
        }
    }
    if (retval > INTERP_MIN_ERROR)
    {
        print_interp_error(retval);
        zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
    }

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanRead() returned %d\n", retval);
    }
    return retval;
}

int zucTaskPlanExecute(const char* command)
{
    int inpos = zucStatus->motion.traj.inpos;  // 1 if in position, 0 if not.

    if (command != 0)
    {  // Command is 0 if in AUTO mode, non-null if in MDI mode.
        // Don't sync if not in position.
        if ((*command != 0) && (inpos))
        {
            interp.synch();
        }
    }
    int retval = interp.execute(command);
    if (retval > INTERP_MIN_ERROR)
    {
        // LogScriptInfo(dynamic_cast<Interp*>(pinterp));
        print_interp_error(retval);
    }
    if (command != 0)
    {
        FINISH();
    }

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanExecute(0) return %d\n", retval);
    }

    return retval;
}

int zucTaskPlanExecute(const char* command, int line_number)
{
    int retval = interp.execute(command, line_number);
    if (retval > INTERP_MIN_ERROR)
    {
        // LogScriptInfo(dynamic_cast<Interp*>(pinterp));
        print_interp_error(retval);
    }
    if (command != 0)
    {  // this means MDI
        FINISH();
    }

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanExecute(%s) returned %d\n", command, retval);
    }

    return retval;
}

int zucTaskPlanClose()
{
    thread_pause_flag = true;
    usleep(1000);
    thread_exit_flag = true;
    // for (auto i = threadStatusList.begin(); i != threadStatusList.end();++i) {
    // 		auto it = (*i).begin();
    // 		pthread_cancel(it->first);
    // 		usleep(1000);
    // }
    threadStatusList.clear();

    int retval = interp.close();
    if (retval > INTERP_MIN_ERROR)
    {
        print_interp_error(retval);
        zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
    }
    zucStatus->task.currentLine = 0;
    taskplanopen = 0;
    interp_list.set_line_number(0);

    for (int i = 0; i < MAX_SUB_THREAD; ++i) zucStatus->sub_thread_state[i][0] = '\0';

    return retval;
}

int zucTaskPlanReset()
{
    int retval = interp.reset();
    if (retval > INTERP_MIN_ERROR)
    {
        print_interp_error(retval);
        zucTaskSetMode(ZUC_TASK_MODE_MANUAL);
    }

    return retval;
}

int zucTaskPlanLine()
{
    int retval = interp.line();

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanLine() returned %d\n", retval);
    }

    return retval;
}

int zucTaskPlanLevel()
{
    int retval = interp.call_level();

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanLevel() returned %d\n", retval);
    }

    return retval;
}

int zucTaskPlanCommand(char* cmd)
{
    char buf[LINELEN];

    strcpy(cmd, interp.command(buf, LINELEN));

    if (zuc_debug & ZUC_DEBUG_INTERP)
    {
        rcs_print("zucTaskPlanCommand(%s) called. (line_number=%d)\n", cmd, zucStatus->task.readLine);
    }

    return 0;
}

int zucTaskUpdate(ZUC_TASK_STAT* stat)
{
    stat->mode = (enum ZUC_TASK_MODE_ENUM)determineMode();
    int oldstate = stat->state;
    stat->state = (enum ZUC_TASK_STATE_ENUM)determineState();

    if (oldstate == ZUC_TASK_STATE_ON && oldstate != stat->state)
    {
        if (zucStatus->motion.category_one_stop_program_pause == 0)
        {
            zucTaskAbort();
            zucAbortCleanup(ZUC_ABORT_TASK_STATE_NOT_ON);
        }
        else
        {
            // zucTrajPause();
            // zucStatus->task.interpState = ZUC_TASK_INTERP_PAUSED;
            // // SET_MOTION_INPOS_FLAG(1);
            // zucStatus->task.task_paused = 1;
            // thread_pause_flag = true;
        }
    }

    if (zucStatus->motion.traj.id > 0)
    {
        stat->motionLine = zucStatus->motion.traj.id;
    }
    stat->errAddLine = zucStatus->motion.errAddLine;

    // if(zucStatus->motion.traj.id == 0 && stat->sleepLine == 0)
    if (zucStatus->motion.traj.inpos == 1 && stat->sleepLine == 0)
    {
        if (taskplanopen == 0)
            zucStatus->task.currentLine = 0;
        // stat->executingLine = zucStatus->task.currentLine;
        // stat->executingLine = zucStatus->task.readLine;
        if (interp_list.len() > 0)
        {
            stat->executingLine = zucStatus->task.currentLine;
        }
        else
        {
            int sequence_number = dynamic_cast<Interp*>(pinterp)->current_line();
            stat->executingLine = zucStatus->task.readLine > sequence_number ? zucStatus->task.readLine : sequence_number;
            // stat->readLine = zucStatus->task.readLine > sequence_number ? zucStatus->task.readLine : sequence_number;
        }
    }
    else
    {
        stat->executingLine = zucStatus->motion.traj.id > stat->sleepLine ? zucStatus->motion.traj.id : stat->sleepLine;
    }

    char buf[LINELEN];
    strcpy(stat->file, interp.file(buf, LINELEN));

    memset(stat->callStack, 0, sizeof(stat->callStack));
    int maxCallStack = sizeof(stat->callStack) / sizeof(ZUC_TASK_STAT::CallStack);
    for (int i = 0; i < maxCallStack; i++)
    {
        if (interp.get_callstack(i, stat->callStack[i].file, &stat->callStack[i].executingLine) != 0)
        {
            strncpy(stat->callStack[i].file, interp.file(buf, LINELEN), sizeof(stat->callStack[i].file));
            stat->callStack[i].executingLine = stat->executingLine;
            break;
        }
    }

    stat->optional_stop_state = GET_OPTIONAL_PROGRAM_STOP();
    stat->block_delete_state = GET_BLOCK_DELETE();
    stat->robotToolOffset = zucStatus->task.robotToolOffset;
    stat->robotUserOffset = zucStatus->task.robotUserOffset;
    stat->backupSetting.enable = zucStatus->task.backupSetting.enable;
    stat->backupSetting.interval = zucStatus->task.backupSetting.interval;

    stat->heartbeat++;
    return 0;
}

int zucTaskTimeoutUpdate()
{
    waitTimeout = zucStatus->task.input_timeout;
    // rcs_print("waitTimeout updated: %d\n", waitTimeout);
    return 0;
}

int zucAbortCleanup(int reason, const char* message)
{
    int status = interp.on_abort(reason, message);
    if (status > INTERP_MIN_ERROR)
        print_interp_error(status);
    return status;
}

int zucTaskSetUserDefinedVar(int id, double value)
{
    int status = interp.set_user_defined_var(id, value);
    if (status > INTERP_MIN_ERROR)
    {
        print_interp_error(status);
        return status;
    }
    return 0;
}

int zucSetMulThreadId(int id)
{
    interp.set_thread_tid(id);
    return 0;
}

int zucSyncZucsettings()
{
    interp.reload_zucsettings();
    return 0;
}
